cmake_minimum_required(VERSION 3.8)
project(chassis_controller)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# option(ENABLE_ASAN "Enable AddressSanitizer" OFF)

# if(ENABLE_ASAN)
#   if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
#     message(STATUS "ASAN enabled")
#     add_compile_options(-fsanitize=address -fno-omit-frame-pointer -g)
#     add_link_options(-fsanitize=address)
#   else()
#     message(WARNING "AddressSanitizer not supported for this compiler")
#   endif()
# endif()

# find dependencies
set(THIS_PACKAGE_INCLUDE_DEPENDS
  control_msgs
  controller_interface
  hardware_interface
  pluginlib
  rclcpp
  rclcpp_lifecycle
  realtime_tools
  std_srvs
  nav_msgs
  tf2
  tf2_msgs
  agv_srvs
  agv_msgs
  geometry_msgs
  tf2_geometry_msgs
  shared_parameters
)

find_package(ament_cmake REQUIRED)
#find_package(generate_parameter_library REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
find_package(controller_manager REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)

foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
  find_package(${Dependency} REQUIRED)
endforeach()

# Add chassis_controller library related compile commands
#generate_parameter_library(chassis_controller_parameters
#  src/chassis_controller.yaml
#  include/chassis_controller/validate_chassis_controller_parameters.hpp
#)

add_library(
  chassis_controller
  SHARED
  src/pid.cpp
  src/speed_limiter.cpp
  src/odometry.cpp
  src/odometry_diff.cpp
  src/odometry_multi_pivots.cpp
  src/odometry_tricycle.cpp
  src/kinematics.cpp
  src/kinematics_diff.cpp
  src/kinematics_multi_pivots.cpp
  src/kinematics_tricycle.cpp
  src/chassis_controller.cpp
  src/odometry_compensation.cpp
  src/current_monitor.cpp
)

target_include_directories(chassis_controller PUBLIC
  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
#target_link_libraries(chassis_controller chassis_controller_parameters)
ament_target_dependencies(chassis_controller ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_compile_definitions(chassis_controller PRIVATE "CHASSIS_CONTROLLER_BUILDING_DLL")

pluginlib_export_plugin_description_file(
  controller_interface chassis_controller.xml)

install(
  TARGETS
  chassis_controller
  RUNTIME DESTINATION bin
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
)

install(
  DIRECTORY include/
  DESTINATION include/${PROJECT_NAME}
)

if(BUILD_TESTING111)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)

  ament_add_gmock(test_load_chassis_controller test/test_load_chassis_controller.cpp)
  target_include_directories(test_load_chassis_controller PRIVATE include)
  ament_target_dependencies(
    test_load_chassis_controller
    controller_manager
    hardware_interface
    ros2_control_test_assets
  )

  add_rostest_with_parameters_gmock(test_chassis_controller test/test_chassis_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chassis_controller_params.yaml)
  target_include_directories(test_chassis_controller PRIVATE include)
  target_link_libraries(test_chassis_controller chassis_controller)
  ament_target_dependencies(
    test_chassis_controller
    controller_interface
    hardware_interface
  )

  add_rostest_with_parameters_gmock(test_chassis_controller_preceeding test/test_chassis_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/chassis_controller_preceeding_params.yaml)
  target_include_directories(test_chassis_controller_preceeding PRIVATE include)
  target_link_libraries(test_chassis_controller_preceeding chassis_controller)
  ament_target_dependencies(
    test_chassis_controller_preceeding
    controller_interface
    hardware_interface
  )
endif()

ament_export_include_directories(
  include
)
ament_export_dependencies(
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)
ament_export_libraries(
  chassis_controller
)

ament_package()
